
// import oimo.m.Mat3;
// import oimo.m.Quat;
// import oimo.m.Vec3;
// import oimo.m.M;

import { Joint } from "./Joint";
import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { Quat } from "../../../common/Quat";
import { Mat3 } from "../../../common/Mat3";

/**
 * numberernal class
 */
export class JointBasis {
	joint: Joint;

	x: Vec3;
	y: Vec3;
	z: Vec3;

	constructor(joint: Joint) {
		this.joint = joint;
		this.x = new Vec3();
		this.y = new Vec3();
		this.z = new Vec3();
	}


	public trackByX(): void {
		this.trackByAxis(0);
	}


	public trackByY(): void {
		this.trackByAxis(1);
	}


	public trackByZ(): void {
		this.trackByAxis(2);
	}


	private trackByAxis(axis: number): void {
		var invM1: number = this.joint._b1._invMass;
		var invM2: number = this.joint._b2._invMass;
		var q: Quat = new Quat();
		var idQ: Quat = new Quat();
		var slerpQ: Quat = new Quat();
		var slerpM: Mat3 = new Mat3();

		var newX: Vec3 = new Vec3();
		var newY: Vec3 = new Vec3();
		var newZ: Vec3 = new Vec3();

		var prevX: Vec3 = new Vec3();
		var prevY: Vec3 = new Vec3();

		if (axis == 0) {
			// compute X'
			M.quat_arc(q, this.joint._basisX1, this.joint._basisX2);
			M.quat_id(idQ);
			M.quat_slerp(slerpQ, idQ, q, invM1 / (invM1 + invM2));
			M.mat3_fromQuat(slerpM, slerpQ);
			M.vec3_mulMat3(newX, this.joint._basisX1, slerpM);

			// set X, Y
			M.vec3_assign(prevX, this.x);
			M.vec3_assign(prevY, this.y);
		} else if (axis == 1) {
			// compute X'
			M.quat_arc(q, this.joint._basisY1, this.joint._basisY2);
			M.quat_id(idQ);
			M.quat_slerp(slerpQ, idQ, q, invM1 / (invM1 + invM2));
			M.mat3_fromQuat(slerpM, slerpQ);
			M.vec3_mulMat3(newX, this.joint._basisY1, slerpM);

			// set X, Y
			M.vec3_assign(prevX, this.y);
			M.vec3_assign(prevY, this.z);
		} else {
			// compute X'
			M.quat_arc(q, this.joint._basisZ1, this.joint._basisZ2);
			M.quat_id(idQ);
			M.quat_slerp(slerpQ, idQ, q, invM1 / (invM1 + invM2));
			M.mat3_fromQuat(slerpM, slerpQ);
			M.vec3_mulMat3(newX, this.joint._basisZ1, slerpM);

			// set X, Y
			M.vec3_assign(prevX, this.z);
			M.vec3_assign(prevY, this.x);
		}

		// we compute Y' and Z' from X, Y, and X' (new basis: <X', Y', Z'>, previous basis: <X, Y, Z>)
		// in following algorithm:
		//   slerp = X -> X'
		//   Y'' = slerp(Y)
		//   Z'' = X' cross Y''
		//   if |Z''| > eps then
		//     Z' = normalize(Z'')
		//   else
		//     Z' = perp(X')         # give up rotating previous basis
		//   Y' = Z' cross X'

		// slerp = X -> X'
		M.quat_arc(slerpQ, prevX, newX);
		M.mat3_fromQuat(slerpM, slerpQ);

		// Y'' = slerp(Y)
		M.vec3_mulMat3(newY, prevY, slerpM);

		// Z'' = X cross Y''
		M.vec3_cross(newZ, newX, newY);

		if (M.vec3_dot(newZ, newZ) > 1e-6) {
			// Z' = normalize(Z'')
			M.vec3_normalize(newZ, newZ);
		} else {
			// failed to rotate previous basis, build a new right-handed orthonormal system
			M.vec3_perp(newZ, newX);
		}

		// Y' = Z' cross X'
		M.vec3_cross(newY, newZ, newX);

		if (axis == 0) {
			M.vec3_assign(this.x, newX);
			M.vec3_assign(this.y, newY);
			M.vec3_assign(this.z, newZ);
		} else if (axis == 1) {
			M.vec3_assign(this.x, newZ);
			M.vec3_assign(this.y, newX);
			M.vec3_assign(this.z, newY);
		} else {
			M.vec3_assign(this.x, newY);
			M.vec3_assign(this.y, newZ);
			M.vec3_assign(this.z, newX);
		}

	}

}
